#include "GLHeaders.hpp"
#include <cmath>
#include "Vector.hpp"
#include "Matrix.hpp"

//Constructor
Matrix::Matrix(void){
	v0.x = (GLfloat)0.0f;
	v0.y = (GLfloat)0.0f;
	v0.z = (GLfloat)0.0f;
	v1.x = (GLfloat)0.0f;
	v1.y = (GLfloat)0.0f;
	v1.z = (GLfloat)0.0f;
	v2.x = (GLfloat)0.0f;
	v2.y = (GLfloat)0.0f;
	v2.z = (GLfloat)0.0f;
}

Matrix::Matrix(
		const GLfloat fa00,
		const GLfloat fa10,
		const GLfloat fa20,
		const GLfloat fa01,
		const GLfloat fa11,
		const GLfloat fa21,
		const GLfloat fa02,
		const GLfloat fa12,
		const GLfloat fa22){
	v0.x = fa00;
	v0.y = fa10;
	v0.z = fa20;
	v1.x = fa01;
	v1.y = fa11;
	v1.z = fa21;
	v2.x = fa02;
	v2.y = fa12;
	v2.z = fa22;
}

Matrix::Matrix(
		const Vector& vector0,
		const Vector& vector1,
		const Vector& vector2){
	v0.x = vector0.x;
	v0.y = vector0.y;
	v0.z = vector0.z;
	v1.x = vector1.x;
	v1.y = vector1.y;
	v1.z = vector1.z;
	v2.x = vector2.x;
	v2.y = vector2.y;
	v2.z = vector2.z;
}

Matrix::Matrix(const Matrix& matrix){
	v0.x = matrix.v0.x;
	v0.y = matrix.v0.y;
	v0.z = matrix.v0.z;
	v1.x = matrix.v1.x;
	v1.y = matrix.v1.y;
	v1.z = matrix.v1.z;
	v2.x = matrix.v2.x;
	v2.y = matrix.v2.y;
	v2.z = matrix.v2.z;
}

//Arithmetic operations
Matrix Matrix::operator + () const{
	return Matrix(*this);
}

Matrix Matrix::operator - () const{
	return Matrix(
			-v0.x,
			-v0.y,
			-v0.z,
			-v1.x,
			-v1.y,
			-v1.z,
			-v2.x,
			-v2.y,
			-v2.z);
}

Matrix Matrix::operator + (const Matrix& rhs) const{
	return Matrix(
			v0.x+rhs.v0.x,
			v0.y+rhs.v0.y,
			v0.z+rhs.v0.z,
			v1.x+rhs.v1.x,
			v1.y+rhs.v1.y,
			v1.z+rhs.v1.z,
			v2.x+rhs.v2.x,
			v1.y+rhs.v2.y,
			v2.z+rhs.v2.z);
}

Matrix Matrix::operator + (const GLfloat& value) const{
	return Matrix(
			v0.x+value,
			v0.y+value,
			v0.z+value,
			v1.x+value,
			v1.y+value,
			v1.z+value,
			v2.x+value,
			v2.y+value,
			v2.z+value);
}

Matrix Matrix::operator - (const Matrix& rhs) const{
	return Matrix(
			v0.x-rhs.v0.x,
			v0.y-rhs.v0.y,
			v0.z-rhs.v0.z,
			v1.x-rhs.v1.x,
			v1.y-rhs.v1.y,
			v1.z-rhs.v1.z,
			v2.x-rhs.v2.x,
			v2.y-rhs.v2.y,
			v2.z-rhs.v2.z);
}

Matrix Matrix::operator - (const GLfloat& value) const{
	return Matrix(
			v0.x-value,
			v0.y-value,
			v0.z-value,
			v1.x-value,
			v1.y-value,
			v1.z-value,
			v2.x-value,
			v2.y-value,
			v2.z-value);
}

Matrix Matrix::operator * (const Matrix& rhs) const{
	return Matrix(
			v0.x*rhs.v0.x+ //1st Vector
			v1.x*rhs.v0.y+
			v2.x*rhs.v0.z,

			v0.y*rhs.v0.x+
			v1.y*rhs.v0.y+
			v2.y*rhs.v0.z,

			v0.z*rhs.v0.x+
			v1.z*rhs.v0.y+
			v2.z*rhs.v0.z,

			v0.x*rhs.v1.x+ //2nd Vector
			v1.x*rhs.v1.y+
			v2.x*rhs.v1.z,

			v0.y*rhs.v1.x+
			v1.y*rhs.v1.y+
			v2.y*rhs.v1.z,

			v0.z*rhs.v1.x+
			v1.z*rhs.v1.y+
			v2.z*rhs.v1.z,

			v0.x*rhs.v2.x+ //3rd Vector
			v1.x*rhs.v2.y+
			v2.x*rhs.v2.z,

			v0.y*rhs.v2.x+
			v1.y*rhs.v2.y+
			v2.y*rhs.v2.z,

			v0.z*rhs.v2.x+
			v1.z*rhs.v2.y+
			v2.z*rhs.v2.z);
}

Vector Matrix::operator * (const Vector& rhs) const{
	return Vector(
			v0.x*rhs.x+ //1st Vector
			v1.x*rhs.y+
			v2.x*rhs.z,

			v0.y*rhs.x+
			v1.y*rhs.y+
			v2.y*rhs.z,

			v0.z*rhs.x+
			v1.z*rhs.y+
			v2.z*rhs.z);
}

Matrix Matrix::operator * (const GLfloat& value) const{
	return Matrix(
			v0.x*value,
			v0.y*value,
			v0.z*value,
			v1.x*value,
			v1.y*value,
			v1.z*value,
			v2.x*value,
			v2.y*value,
			v2.z*value);
}

Matrix Matrix::operator / (const Matrix& rhs) const{
	return Matrix((*this)*rhs.Inverse());
}

Matrix Matrix::operator / (const GLfloat& value) const{
	return Matrix(
			v0.x/value,
			v0.y/value,
			v0.z/value,
			v1.x/value,
			v1.y/value,
			v1.z/value,
			v2.x/value,
			v2.y/value,
			v2.z/value);
}

//Friend Methods
Matrix operator + (const GLfloat& value, const Matrix& rhs){
	return Matrix(
			value+rhs.v0.x,
			value+rhs.v0.y,
			value+rhs.v0.z,
			value+rhs.v1.x,
			value+rhs.v1.y,
			value+rhs.v1.z,
			value+rhs.v2.x,
			value+rhs.v2.y,
			value+rhs.v2.z);
}

Matrix operator - (const GLfloat& value, const Matrix& rhs){
	return Matrix(
			value-rhs.v0.x,
			value-rhs.v0.y,
			value-rhs.v0.z,
			value-rhs.v1.x,
			value-rhs.v1.y,
			value-rhs.v1.z,
			value-rhs.v2.x,
			value-rhs.v2.y,
			value-rhs.v2.z);
}

Matrix operator * (const GLfloat& value, const Matrix& rhs){
	return Matrix(
			value*rhs.v0.x,
			value*rhs.v0.y,
			value*rhs.v0.z,
			value*rhs.v1.x,
			value*rhs.v1.y,
			value*rhs.v1.z,
			value*rhs.v2.x,
			value*rhs.v2.y,
			value*rhs.v2.z);
}

Matrix operator / (const GLfloat& value, const Matrix& rhs){
	return Matrix(
			value/rhs.v0.x,
			value/rhs.v0.y,
			value/rhs.v0.z,
			value/rhs.v1.x,
			value/rhs.v1.y,
			value/rhs.v1.z,
			value/rhs.v2.x,
			value/rhs.v2.y,
			value/rhs.v2.z);
}

//Boolean operation
bool Matrix::operator < (const Matrix& rhs) const{
	if( v0.x < rhs.v0.x&&
			v0.y < rhs.v0.y&&
			v0.z < rhs.v0.z&&
			v1.x < rhs.v1.x&&
			v1.y < rhs.v1.y&&
			v1.z < rhs.v1.z&&
			v2.x < rhs.v2.x&&
			v2.y < rhs.v2.y&&
			v2.z < rhs.v2.z)
		return true;
	else
		return false;
}

bool Matrix::operator <= (const Matrix& rhs) const{
	if( v0.x <= rhs.v0.x&&
			v0.y <= rhs.v0.y&&
			v0.z <= rhs.v0.z&&
			v1.x <= rhs.v1.x&&
			v1.y <= rhs.v1.y&&
			v1.z <= rhs.v1.z&&
			v2.x <= rhs.v2.x&&
			v2.y <= rhs.v2.y&&
			v2.z <= rhs.v2.z)
		return true;
	else
		return false;
}

bool Matrix::operator == (const Matrix& rhs) const{
	if( v0.x == rhs.v0.x&&
			v0.y == rhs.v0.y&&
			v0.z == rhs.v0.z&&
			v1.x == rhs.v1.x&&
			v1.y == rhs.v1.y&&
			v1.z == rhs.v1.z&&
			v2.x == rhs.v2.x&&
			v2.y == rhs.v2.y&&
			v2.z == rhs.v2.z)
		return true;
	else
		return false;
}  

bool Matrix::operator != (const Matrix& rhs) const{
	if( v0.x != rhs.v0.x&&
			v0.y != rhs.v0.y&&
			v0.z != rhs.v0.z&&
			v1.x != rhs.v1.x&&
			v1.y != rhs.v1.y&&
			v1.z != rhs.v1.z&&
			v2.x != rhs.v2.x&&
			v2.y != rhs.v2.y&&
			v2.z != rhs.v2.z)
		return true;
	else
		return false;
}

bool Matrix::operator >= (const Matrix& rhs) const{
	if( v0.x >= rhs.v0.x&&
			v0.y >= rhs.v0.y&&
			v0.z >= rhs.v0.z&&
			v1.x >= rhs.v1.x&&
			v1.y >= rhs.v1.y&&
			v1.z >= rhs.v1.z&&
			v2.x >= rhs.v2.x&&
			v2.y >= rhs.v2.y&&
			v2.z >= rhs.v2.z)
		return true;
	else
		return false;
}

bool Matrix::operator > (const Matrix& rhs) const{
	if( v0.x > rhs.v0.x&&
			v0.y > rhs.v0.y&&
			v0.z > rhs.v0.z&&
			v1.x > rhs.v1.x&&
			v1.y > rhs.v1.y&&
			v1.z > rhs.v1.z&&
			v2.x > rhs.v2.x&&
			v2.y > rhs.v2.y&&
			v2.z > rhs.v2.z)
		return true;
	else
		return false;
}

//Arithmetic updates
Matrix& Matrix::operator = (const Matrix& rhs){
	v0.x = rhs.v0.x;
	v0.y = rhs.v0.y;
	v0.z = rhs.v0.z;
	v1.x = rhs.v1.x;
	v1.y = rhs.v1.y;
	v1.z = rhs.v1.z;
	v2.x = rhs.v2.x;
	v2.y = rhs.v2.y;
	v2.z = rhs.v2.z;
	return *this;
}

Matrix& Matrix::operator = (const GLfloat& value){
	v0.x = value;
	v0.y = value;
	v0.z = value;
	v1.x = value;
	v1.y = value;
	v1.z = value;
	v2.x = value;
	v2.y = value;
	v2.z = value;
	return *this;
}

Matrix& Matrix::operator += (const Matrix& rhs){
	*this = *this + rhs;
	return *this;
}

Matrix& Matrix::operator += (const GLfloat& value){
	*this = *this + value;
	return *this;
}

Matrix& Matrix::operator -= (const Matrix& rhs){
	*this = *this - rhs;
	return *this;
}

Matrix& Matrix::operator -= (const GLfloat& value){
	*this = *this - value;
	return *this;
}

Matrix& Matrix::operator *= (const Matrix& rhs){
	*this = *this * rhs;
	return *this;
}

Matrix& Matrix::operator *= (const GLfloat& value){
	*this = *this * value;
	return *this;
}

Matrix& Matrix::operator /= (const Matrix& rhs){
	*this = *this / rhs;
	return *this;
}

Matrix& Matrix::operator /= (const GLfloat& value){
	*this = *this / value;
	return *this;
}

//Functions
//m11 m22 m33 +
//m12 m23 m31 +
//m13 m21 m32 -
//m11 m23 m32 -
//m12 m 21 m33 -
//m13 m22 m31
GLfloat Matrix::Determinant(void) const{
	return(
		v0.x * v1.y * v2.z +
		v1.x * v2.y * v0.z +
		v2.x * v0.y * v1.z -
		v0.x * v2.y * v1.z -
		v1.x * v0.y * v2.z -
		v2.x * v1.y * v0.z);
}

//Suppose Matrix is inversible!!! (det != 0)
Matrix Matrix::Inverse(void) const{
	GLfloat idet = 1.0f/this->Determinant();
	Matrix result;
	// Vector 0
	// m11*m22 - m12*m21	
	result.v1.x = (v1.y*v2.z - v2.y*v1.z) * idet;
	// m12*m20 - m10*m22	
	result.v1.y = (v2.y*v0.z - v0.y*v2.z) * idet;
	// m10*m21 - m11*m20	
	result.v1.z = (v0.y*v1.z - v1.y*v0.z) * idet;
	// Vector 1
	// 	m02*m21 - m01*m22
	result.v2.x = (v2.x*v1.z - v1.x*v2.z) * idet;
	// m00*m22 - m02*m20
	result.v2.y = (v0.x*v2.z - v2.x*v0.z) * idet;
	// m01*m20 - m00*m21
	result.v2.z = (v1.x*v0.z - v0.x*v1.z) * idet;
	// Vector 2
	// m01*m12 - m02*m11
	result.v2.x = (v1.x*v2.y - v2.x*v1.y) * idet;
	// m02*m10 - m00*m12
	result.v2.y = (v2.x*v0.y - v0.x*v2.y) * idet;
	// m00*m11 - m01*m10
	result.v2.z = (v0.x*v1.y - v1.x*v0.y) * idet;
	return result;
}

Matrix Matrix::Transpose(void) const{
	return Matrix(
			v0.x,
			v1.x,
			v2.x,
			v0.y,
			v1.y,
			v2.y,
			v0.z,
			v1.z,
			v2.z);		  
}

void Matrix::Copy3f(GLfloat* tab) const{
	*tab     = v0.x;//column 1
	*(tab+1) = v0.y;
	*(tab+2) = v0.z;
	*(tab+3) = v1.x;//column 2
	*(tab+4) = v1.y;
	*(tab+5) = v1.z;
	*(tab+6) = v2.x;//column 3
	*(tab+7) = v2.y;
	*(tab+8) = v2.z;
}

void Matrix::Copy4f(GLfloat* tab) const{
	*tab      = v0.x;//column 1
	*(tab+1)  = v0.y;
	*(tab+2)  = v0.z;
	*(tab+3)  = 0.0f;
	*(tab+4)  = v1.x;//column 2
	*(tab+5)  = v1.y;
	*(tab+6)  = v1.z;
	*(tab+7)  = 0.0f;
	*(tab+8)  = v2.x;//column 4
	*(tab+9)  = v2.y;
	*(tab+10) = v2.z;
	*(tab+11) = 0.0f;
	*(tab+12) = 0.0f;//column 4
	*(tab+13) = 0.0f;
	*(tab+14) = 0.0f;
	*(tab+15) = 1.0f;
}

void Matrix::glRotate(void) const{
	GLfloat array[16];
	this->Copy4f(array);
	glMultMatrixf(array);
}

Matrix Matrix::Rotate(
		const GLfloat fangle,
		const GLfloat fx,
		const GLfloat fy,
		const GLfloat fz){
	if((fangle == 0.0f) || (fx == 0.0f && fy == 0.0f && fz == 0.0f))
		return Matrix(Matrix::IDENTITY);

	GLfloat s = sinf(fangle * M_PI / 180.0f);
	GLfloat c = cosf(fangle * M_PI / 180.0f); 
	Vector vecR(fx,fy,fz);
	vecR = vecR.Normalize();
	
	Matrix matR(
			// 1st column
			vecR.x*vecR.x*(1-c)+c,
			vecR.y*vecR.x*(1-c)+vecR.z*s,
			vecR.x*vecR.z*(1-c)-vecR.y*s,
		
			//2nd column
			vecR.x*vecR.y*(1-c)-vecR.z*s,
			vecR.y*vecR.y*(1-c)+c,
			vecR.y*vecR.z*(1-c)+vecR.x*s,
	
			//3rd column
			vecR.x*vecR.z*(1-c)+vecR.y*s,
			vecR.y*vecR.z*(1-c)-vecR.x*s,
			vecR.z*vecR.z*(1-c)+c);

	return matR;
}

//Special matrix
const Matrix Matrix::ZERO(
		0.0f, 0.0f, 0.0f,
		0.0f, 0.0f, 0.0f,
		0.0f, 0.0f, 0.0f);

const Matrix Matrix::ONES(
		1.0f, 1.0f, 1.0f,
		1.0f, 1.0f, 1.0f,
		1.0f, 1.0f, 1.0f);

const Matrix Matrix::IDENTITY( 
		1.0f, 0.0f, 0.0f,
		0.0f, 1.0f, 0.0f,
		0.0f, 0.0f, 1.0f);

const Matrix Matrix::RX_90( 
		1.0f, 0.0f, 0.0f,
		0.0f, 0.0f, 1.0f,
		0.0f,-1.0f, 0.0f); 

const Matrix Matrix::RY_90(
		0.0f, 0.0f,-1.0f,
		0.0f, 1.0f, 0.0f,
		1.0f, 0.0f, 0.0f); 

const Matrix Matrix::RZ_90(
		 0.0f, 1.0f, 0.0f,
		-1.0f, 0.0f, 0.0f,
		 0.0f, 0.0f, 1.0f); 

const Matrix Matrix::NEGATIVE_RX_90(
		1.0f, 0.0f, 0.0f, 
		0.0f, 0.0f,-1.0f,
		0.0f, 1.0f, 0.0f); 

const Matrix Matrix::NEGATIVE_RY_90(
		 0.0f, 0.0f, 1.0f,
		 0.0f, 1.0f, 0.0f,
		-1.0f, 0.0f, 0.0f);

const Matrix Matrix::NEGATIVE_RZ_90(
		0.0f,-1.0f, 0.0f,
		1.0f, 0.0f, 0.0f,
		0.0f, 0.0f, 1.0f);

